/**
 * @file Dog.c
 * @brief
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2021-10-27
 *
 * @copyright Copyright (c) 2021  北京邮电大学
 *
 * @par 修改日志:
 * @date 2021-10-27
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */
#ifdef 0
#include "Dog.h"
#include "utils.h"
Dog_s Dog;
Time_s Dog_time = {0, 0, 0};

float dog_foot_coordinate[3][4];
float delta_h = 0;
float delta_z = 0;
float delta_t = 0;
float delta_ds = 0;
float delta_dth = 0;
float delta_Dth = 0;
float delta_dz = 0;
float dz_k = 5;
float delta_dz1 = 0;
float delta_dh = 0;
//上楼梯参数dog adjust 0.08 0.05 4 0.02 0 0.02 0.135 0

float barycenter_front = 0.0795;
float barycenter_back = 0;
float barycenter_left = 0.0803;
float barycenter_right = 0.0803;
float barycenter_central = 0.0268;

float A = 95 * PI / 180; //转弯角度
float A1 = 0.11;
float B1 = 0.0415;         //关节1、2的x距离
float B2 = 0.0776;         //不同腿关节1的x距离
float LL = 0.1756 - 0.015; //左右腿间距
float R0 = 0.4;            //相对于身体中心的弯道半径
float Dth = 10 * PI / 180; //一个周期实现的转向角
float dth = 3 * PI / 180;  //一个周期实现的转弯角
void Dog_init()
{
    Dog.legs[0].servo[0].ID = 0;
    Dog.legs[0].servo[1].ID = 1;
    Dog.legs[0].servo[2].ID = 2;
    Dog.legs[0].No = 0;

    Dog.legs[1].servo[0].ID = 10;
    Dog.legs[1].servo[1].ID = 11;
    Dog.legs[1].servo[2].ID = 12;
    Dog.legs[1].No = 1;

    Dog.legs[2].servo[0].ID = 20;
    Dog.legs[2].servo[1].ID = 21;
    Dog.legs[2].servo[2].ID = 22;
    Dog.legs[2].No = 2;

    Dog.legs[3].servo[0].ID = 30;
    Dog.legs[3].servo[1].ID = 31;
    Dog.legs[3].servo[2].ID = 32;
    Dog.legs[3].No = 3;

    Dog.ctrl_mode = DOG_MODE_NONE;
    Dog.upstair_flag = 0;
}

void Dog_exe()
{
    switch (Dog.ctrl_mode)
    {
    case DOG_MODE_NONE:
    {
        break;
    }
    case DOG_MODE_STOP:
    {
        Dog_Gait_Reset();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        Dog.ctrl_mode = DOG_MODE_NONE;
        break;
    }
    case DOG_MODE_RESET:
    {
        Dog_Gait_Reset();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        Dog.ctrl_mode = DOG_MODE_NONE;
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_RUN:
    {
        // static uint64_t tick = 0;

        // Dog_Gait_1(tick);
        // Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time1:%f \n\r", Dog_time.s);
        // osDelay(200);
        Dog_Gait_2();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        // osDelay(200);
        // tick++;
        break;
    }
    case DOG_MODE_TURN11:
    {
        Dog_Gait_zhuanxiang1_1();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_TURN21:
    {
        Dog_Gait_zhuanxiang2_1();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_TURN12:
    {
        Dog_Gait_zhuanxiang1_2();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }

    case DOG_MODE_TURN22:
    {
        Dog_Gait_zhuanxiang2_2();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_LEFT:
    {
        Dog_Gait_zuozhuanwan();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_RIGHT:
    {
        Dog_Gait_youzhuanwan();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_DOWN:
    {
        Dog_Gait_xiapo();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_UPSTAIR:
    {
        // Do_Gait_shangtaijie();
        if (Dog.upstair_flag == 0 && Dog_time.s < (GAIT_T + delta_t) / 2)
        {
            uprintf("\n\rzuoyi\n\r");
            Dog_Gait_zhongxinzuoyi();
        }
        else if (Dog.upstair_flag == 0 && Dog_time.s >= (GAIT_T + delta_t) / 2)
        {
            float T = (GAIT_T + delta_t);
            get_tick(&Dog_time);
            float t = fmod((Dog_time.s - T), T);
            uprintf("Dogtime %f\n\r", t);
            Dog.upstair_flag = 1;
            reset_time(&Dog_time);
            get_tick(&Dog_time);
            t = fmod((Dog_time.s - T), T);
            uprintf("reset Dogtime %f\n\r", t);
        }
        if (Dog.upstair_flag == 1)
        {
            Dog_Gait_shangtaijie_zuoyou();
        }

        Servo_Sync_Goal_Position(broadcast_ctrl_pos);

        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_SQUARE:
    {
        Do_Gait_fangxingbutai();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);

        // show_broadcast();
        // uprintf("time2:%f \n\r", Dog_time.s);
        break;
    }
    case DOG_MODE_DEBUG:
    {
        // Dog_Inverse_kinematics();
        Servo_Sync_Torque(1);
        // Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        // Dog_Gait_centroid_forward();
        // Dog_Gait_shangtaijie_zuoyou();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        Dog.ctrl_mode = DOG_MODE_NONE;
        break;
    }
    default:
    {
        break;
    }
    }
}

void Dog_Gait_Reset()
{
    Dog.legs[0].foot_target_x = LEG0_INIT_X;
    Dog.legs[0].foot_target_y = LEG0_INIT_Y;
    Dog.legs[0].foot_target_z = LEG0_INIT_Z;

    Dog.legs[1].foot_target_x = LEG1_INIT_X;
    Dog.legs[1].foot_target_y = LEG1_INIT_Y;
    Dog.legs[1].foot_target_z = LEG1_INIT_Z;

    Dog.legs[2].foot_target_x = LEG2_INIT_X;
    Dog.legs[2].foot_target_y = LEG2_INIT_Y;
    Dog.legs[2].foot_target_z = LEG2_INIT_Z;

    Dog.legs[3].foot_target_x = LEG3_INIT_X;
    Dog.legs[3].foot_target_y = LEG3_INIT_Y;
    Dog.legs[3].foot_target_z = LEG3_INIT_Z;

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_1()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);

    get_tick(&Dog_time);
    float t = Dog_time.s - T;

    // float t = tick / 100.0;
    Dog.legs[0].foot_target_x = LEG0_INIT_X;
    Dog.legs[0].foot_target_y = LEG0_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
    Dog.legs[0].foot_target_z = LEG0_INIT_Z + S * 0.5 * (1 - cos(t * PI / (T * 0.5)));

    Dog.legs[1].foot_target_x = LEG1_INIT_X;
    Dog.legs[1].foot_target_y = LEG1_INIT_Y;
    Dog.legs[1].foot_target_z = LEG1_INIT_Z - (S + DS) * 0.5 * (0.5 - cos(t * PI / (T * 0.5)));

    Dog.legs[2].foot_target_x = LEG2_INIT_X;
    Dog.legs[2].foot_target_y = LEG2_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
    Dog.legs[2].foot_target_z = LEG2_INIT_Z + S * 0.5 * (1 - cos(t * PI / (T * 0.5)));

    Dog.legs[3].foot_target_x = LEG3_INIT_X;
    Dog.legs[3].foot_target_y = LEG3_INIT_Y;
    Dog.legs[3].foot_target_z = LEG3_INIT_Z - (S + DS) * 0.5 * (1 - cos(t * PI / (T * 0.5)));

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 复位
 */
void Dog_Gait_Reset()
{
    float T = (GAIT_T + delta_t);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - 1.5 * T), T);
    float x010 = LEG1_INIT_X, x10 = LEG1_INIT_X;
    float y010 = LEG1_INIT_Y, y10 = LEG1_INIT_Y;
    float z010 = LEG1_INIT_Z, z10 = LEG1_INIT_Z;
    float x020 = LEG0_INIT_X, x20 = LEG0_INIT_X;
    float y020 = LEG0_INIT_Y, y20 = LEG0_INIT_Y;
    float z020 = LEG0_INIT_Z, z20 = LEG0_INIT_Z;
    float x030 = LEG3_INIT_X, x30 = LEG3_INIT_X;
    float y030 = LEG3_INIT_Y, y30 = LEG3_INIT_Y;
    float z030 = LEG3_INIT_Z, z30 = LEG3_INIT_Z;
    float x040 = LEG2_INIT_X, x40 = LEG2_INIT_X;
    float y040 = LEG2_INIT_Y, y40 = LEG2_INIT_Y;
    float z040 = LEG2_INIT_Z, z40 = LEG2_INIT_Z;

    Dog.legs[0].foot_target_x = x010 + (x10 - x010) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[0].foot_target_y = y010 + (y10 - y010) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[0].foot_target_z = z010 + (z10 - z010) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_x = x020 + (x20 - x020) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_y = y020 + (y20 - y020) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_z = z020 + (z20 - z020) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_x = x030 + (x30 - x030) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_y = y030 + (y30 - y030) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_z = z030 + (z30 - z030) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_x = x040 + (x40 - x040) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_y = y040 + (y40 - y040) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_z = z040 + (z40 - z040) / 2 * (1 - cos(t * PI / (T / 2)));

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 直走
 */
void Dog_Gait_2()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);

    get_tick(&Dog_time);
    // float t = (Dog_time.s - 1.5 * (GAIT_T+ delta_t)) % (GAIT_T+ delta_t);
    float t = fmod((Dog_time.s - 1.5 * T), T);
    // float t = tick / 100.0;
    if (t < T * 0.5)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (2 * S + DS) * 0.5 * cos((t - T * 0.5) * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (2 * S + DS) * 0.5 * cos(t * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (2 * S + DS) * 0.5 * cos((t - T * 0.5) * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (2 * S + DS) * 0.5 * cos(t * PI / (T * 0.5)) - GAIT_DS * 0.5;
    }
    else
    {
        t = t - T * 0.5;

        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (2 * S + DS) * 0.5 * cos(t * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (2 * S + DS) * 0.5 * cos((t + T * 0.5) * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (2 * S + DS) * 0.5 * cos(t * PI / (T * 0.5)) - GAIT_DS * 0.5;

        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (2 * S + DS) * 0.5 * cos((t + T * 0.5) * PI / (T * 0.5)) - GAIT_DS * 0.5;
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_3()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);

    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - 1.5 * T), T);
    Dog.legs[0].foot_target_x = LEG1_INIT_X;
    Dog.legs[0].foot_target_y = LEG1_INIT_Y;
    Dog.legs[0].foot_target_z = LEG1_INIT_Z - (S + DS) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.25)));

    Dog.legs[1].foot_target_x = LEG0_INIT_X;
    Dog.legs[1].foot_target_y = LEG0_INIT_Y - H * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.25)));
    Dog.legs[1].foot_target_z = LEG0_INIT_Z + S * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));

    Dog.legs[2].foot_target_x = LEG3_INIT_X;
    Dog.legs[2].foot_target_y = LEG3_INIT_Y;
    Dog.legs[2].foot_target_z = LEG3_INIT_Z - (S + DS) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));

    Dog.legs[3].foot_target_x = LEG2_INIT_X;
    Dog.legs[3].foot_target_y = LEG2_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.25)));
    Dog.legs[3].foot_target_z = LEG2_INIT_Z + S * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 左转向 转有限个Dth
 */
void Dog_Gait_zhuanxiang1_1()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);
    float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float th00 = atan(A1 / (B1 + B2));
    float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
    float Z1 = LL * sin(th00 + Dth);
    float X1 = LL * cos(th00 + Dth);
    float Dx1 = -(X1 - B1 - B2); //>0
    float Dz1 = Z1 - A1;
    float Z2 = LL * sin(th00 - Dth);
    float X2 = LL * cos(th00 - Dth);
    float Dx2 = X2 - B1 - B2; //>0
    float Dz2 = -(Z2 - A1);   //>0
    float H1 = H - 0.0;

    if (t <= T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (-Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z;
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z;
    }
    else if (t > T / 4 && t <= T / 2)
    {
        t = t - (T / 4);
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - Dz2;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz2;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}
/**
 * @brief 左转向 转A对Dth的余数
 */
void Dog_Gait_zhuanxiang1_2()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);
    float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float Dth_1 = (fmod((A / (PI / 180)), (Dth / (PI / 180)))) * PI / 180;
    float th00 = atan(A1 / (B1 + B2));
    float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
    float Z1 = LL * sin(th00 + Dth_1);
    float X1 = LL * cos(th00 + Dth_1);
    float Dx1 = -(X1 - B1 - B2); // > 0
    float Dz1 = Z1 - A1;
    float Z2 = LL * sin(th00 - Dth_1);
    float X2 = LL * cos(th00 - Dth_1);
    float Dx2 = X2 - B1 - B2; // > 0
    float Dz2 = -(Z2 - A1);
    float H1 = H - 0.0; // > 0
    if (t < T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (-Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z;
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z;
    }
    else if (t > T / 4 && t < T / 2)
    {
        t = t - (T / 4);
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - Dz2;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz2;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}
/**
 * @brief 右转向 转有限个Dth
 */
void Dog_Gait_zhuanxiang2_1()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);
    float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float th00 = atan(A1 / (B1 + B2));
    float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
    float Z1 = LL * sin(th00 - Dth);
    float X1 = LL * cos(th00 - Dth);
    float Dx1 = (X1 - B1 - B2); //>0
    float Dz1 = -(Z1 - A1);
    float Z2 = LL * sin(th00 + Dth);
    float X2 = LL * cos(th00 + Dth);
    float Dx2 = -(X2 - B1 - B2); //>0
    float Dz2 = (Z2 - A1);
    float H1 = H - 0.0;
    if (t <= T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    else if (t > T / 4 && t <= T / 2)
    {
        t = t - (T / 4);
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (-Dz1);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz1);
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}
/**
 * @brief 右转向 转A对Dth的余数
 */
void Dog_Gait_zhuanxiang2_2()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);
    float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float Dth_1 = (fmod((A / (PI / 180)), (Dth / (PI / 180)))) * PI / 180;
    float th00 = atan(A1 / (B1 + B2));
    float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
    float Z1 = LL * sin(th00 - Dth_1);
    float X1 = LL * cos(th00 - Dth_1);
    float Dx1 = (X1 - B1 - B2); //>0
    float Dz1 = -(Z1 - A1);
    float Z2 = LL * sin(th00 + Dth_1);
    float X2 = LL * cos(th00 + Dth_1);
    float Dx2 = -(X2 - B1 - B2); //>0
    float Dz2 = (Z2 - A1);
    float H1 = H - 0.0;

    if (t <= T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    else if (t > T / 4 && t <= T / 2)
    {
        t = t - (T / 4);

        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (-Dz1);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz1);
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_zuozhuanwan()
{
    float T = (GAIT_T + delta_t);
    // float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    // float DS = (GAIT_DS + delta_ds);
    // float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    // float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float H1 = H - 0.0;
    float X10 = R0 + LL / 2;
    float Y10 = A1;
    float X20 = R0 - LL / 2;
    float Y20 = A1;
    float X30 = R0 + LL / 2;
    float Y30 = -A1;
    float X40 = R0 - LL / 2;
    float Y40 = -A1;
    float ll_1 = sqrt(pow(X10, 2) + pow(Y10, 2));
    float ll_2 = sqrt(pow(X20, 2) + pow(Y20, 2));
    float ll_3 = sqrt(pow(X30, 2) + pow(Y30, 2));
    float ll_4 = sqrt(pow(X40, 2) + pow(Y40, 2));
    float th10 = atan(abs((Y10) / X10));
    float th20 = atan(abs((Y20) / X20));
    float th30 = -atan(abs((Y30) / X30));
    float th40 = -atan(abs((Y40) / X40));
    float X1 = ll_1 * cos(th10 + dth);
    float Y1 = ll_1 * sin(th10 + dth);
    float X2 = ll_2 * cos(th20 + dth);
    float Y2 = ll_2 * sin(th20 + dth);
    float X3 = ll_3 * cos(th30 + dth);
    float Y3 = ll_3 * sin(th30 + dth);
    float X4 = ll_4 * cos(th40 + dth);
    float Y4 = ll_4 * sin(th40 + dth);
    float Dx1 = -(X1 - X10);
    float Dz1 = (Y1 - Y10);
    float Dx2 = -(X2 - X20);
    float Dz2 = (Y2 - Y20);
    float Dx3 = -(X3 - X30);
    float Dz3 = (Y3 - Y30);
    float Dx4 = -(X4 - X40);
    float Dz4 = (Y4 - Y40);
    float dox = -(Dx1 + Dx4) / 2;
    float doz = (Dz1 + Dz4) / 2 * 1.2;

    if (t <= T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X - (dox) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z - (doz) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (Dz3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X - (dox) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - (doz) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z;
    }
    else if (t > T / 4 && t <= T / 2)
    {
        t = t - (T / 4);
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X - (dox) + (Dx1 + (dox)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z - (doz) + (Dz1 + (doz)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx3);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (Dz3);
        Dog.legs[3].foot_target_x = LEG2_INIT_X - (dox) + (Dx4 + (dox)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z - (doz) + (Dz4 + (doz)) / 2 * (1 - cos((t * PI / (T / 4))));
    }
    else if (t > T / 2 && t <= 3 * T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1;
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx3;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz3;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx4;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz4;
    }
    else
    {
        t = t - 3 * T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx3 - (Dx3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz3 - (Dz3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx4 - (Dx4) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz4 - (Dz4) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_youzhuanwan()
{
    float T = (GAIT_T + delta_t);
    // float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    // float DS = (GAIT_DS + delta_ds);
    // float Dth = (GAIT_DTH + delta_Dth);
    float dth = (GAIT_dth + delta_dth);
    //  float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float H1 = H - 0.0;
    float X10 = -R0 + LL / 2;
    float Y10 = A1;
    float X20 = -R0 - LL / 2;
    float Y20 = A1;
    float X30 = -R0 + LL / 2;
    float Y30 = -A1;
    float X40 = -R0 - LL / 2;
    float Y40 = -A1;
    float ll_1 = sqrt(pow(X10, 2) + pow(Y10, 2));
    float ll_2 = sqrt(pow(X20, 2) + pow(Y20, 2));
    float ll_3 = sqrt(pow(X30, 2) + pow(Y30, 2));
    float ll_4 = sqrt(pow(X40, 2) + pow(Y40, 2));
    float th10 = atan(abs((Y10) / X10));
    float th20 = atan(abs((Y20) / X20));
    float th30 = -atan(abs((Y30) / X30));
    float th40 = -atan(abs((Y40) / X40));
    float X1 = -ll_1 * cos(th10 + dth);
    float Y1 = ll_1 * sin(th10 + dth);
    float X2 = -ll_2 * cos(th20 + dth);
    float Y2 = ll_2 * sin(th20 + dth);
    float X3 = -ll_3 * cos(th30 + dth);
    float Y3 = ll_3 * sin(th30 + dth);
    float X4 = -ll_4 * cos(th40 + dth);
    float Y4 = ll_4 * sin(th40 + dth);
    float Dx1 = -(X1 - X10);
    float Dz1 = (Y1 - Y10);
    float Dx2 = -(X2 - X20);
    float Dz2 = (Y2 - Y20);
    float Dx3 = -(X3 - X30);
    float Dz3 = (Y3 - Y30);
    float Dx4 = -(X4 - X40);
    float Dz4 = (Y4 - Y40);
    float dox = -(Dx1 + Dx4) / 2;
    float doz = (Dz1 + Dz4) / 2;

    if (t <= T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X - (dox) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - (doz) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X - (dox) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z - (doz) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx4) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz4) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    else if (t > T / 4 && t <= T / 2)
    {
        t = t - (T / 4);
        Dog.legs[0].foot_target_x = LEG1_INIT_X - (dox) + (Dx2 + (dox)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z - (doz) + (Dz2 + (doz)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (Dx1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1);
        Dog.legs[2].foot_target_x = LEG3_INIT_X - (dox) + (Dx3 + (dox)) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z - (doz) + (Dz3 + (doz)) / 2 * (1 - cos((t * PI / (T / 4))));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx4);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (Dz4);
    }
    else if (t > T / 2 && t <= 3 * T / 4)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2;
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1;
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx3;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz3;
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx4;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz4;
    }
    else
    {
        t = t - 3 * T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx3 - (Dx3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + Dz3 - (Dz3) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx4 - (Dx4) / 2 * (1 - cos((t)*PI / (T / 4)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + Dz4 - (Dz4) / 2 * (1 - cos((t)*PI / (T / 4)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_xiapo()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    //    float DS = (GAIT_DS + delta_ds);
    // float Dth = (GAIT_DTH + delta_Dth);
    //  float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);

    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float H1 = H - 0.0;
    float dz_1 = 3 * dz;      //重心后移，系数可调
    float dz_2 = 5 * dz;      //重心前移，系数可调
    float dz_3 = dz_2 - dz_1; //复位

    if (t < T / 8)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 8 && t < T / 8 + T / 8)
    {
        t = t - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 16)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 && t < T / 4 + T / 8)
    {
        t = t - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 16)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 8 && t < T / 2)
    {
        t = t - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 2 && t < T / 2 + T / 8)
    {
        t = t - T / 2;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 16)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 8 && t < T / 2 + T / 4)
    {
        t = t - T / 2 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 16)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + (S) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else
    {
        t = t - T / 2 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2) + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 4)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2) + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 4)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 4)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + S + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 4)));
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

/**
 * @brief 方形步态
 */
void Do_Gait_fangxingbutai()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    float DS = (GAIT_DS + delta_ds);
    //   float Dth = (GAIT_DTH + delta_Dth);
    //   float dth = (GAIT_dth + delta_dth);
    //  float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);
    float H1 = H - 0.0;
    float dz_1 = DS;
    float dz_2 = 2 * DS;
    float dz_3 = dz_2 - dz_1;

    if (t < T / 8)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 8 && t < T / 8 + T / 24)
    {
        t = t - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 8 + T / 24 && t < T / 8 + T / 12)
    {
        t = t - T / 8 - T / 24;

        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 8 + T / 12 && t < T / 8 + T / 8)
    {
        t = t - T / 8 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 && t < T / 4 + T / 24)
    {
        t = t - T / 4;

        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
    {
        t = t - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
    {
        t = t - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S;
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 8 && t < T / 2)
    {
        t = t - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 2 && t < T / 2 + T / 24)
    {
        t = t - T / 2;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 24 && t < T / 2 + T / 12)
    {
        t = t - T / 2 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 12 && t < T / 2 + T / 8)
    {
        t = t - T / 2 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 8 && t < T / 2 + T / 8 + T / 24)
    {
        t = t - T / 2 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 8 + T / 24 && t < T / 2 + T / 8 + T / 12)
    {
        t = t - T / 2 - T / 24 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
    }
    else if (t >= T / 2 + T / 8 + T / 12 && t < T / 2 + T / 4)
    {
        t = t - T / 2 - T / 8 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + S;
    }
    else if (t >= T / 2 + T / 4 && t < T / 2 + T / 4 + T / 8)
    {
        t = t - T / 2 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (dz_2) + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (dz_2) + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + S + (-S + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z;
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z;
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}
/**
 * @brief 上台阶
 */
void Do_Gait_shangtaijie()
{
    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    //   float DS = (GAIT_DS + delta_ds);
    //   float Dth = (GAIT_DTH + delta_Dth);
    //   float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);

    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float h = 60 / 10 * 1.1;
    float H1 = H + delta_dh;
    float dz_1 = barycenter_back;  // dz;          //重心后移，系数可调
    float dz_2 = barycenter_front; // dz_k * dz;   //重心前移，系数可调
    float dz_3 = dz_2 - dz_1;      //复位

    float K_H1 = 1.0;
    float K_S = 1.0;
    if (t < T / 8)
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 8 && t < T / 8 + T / 24)
    {
        t = t - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - K_H1 * H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1)-H1 / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 8 + T / 24 && t < T / 8 + T / 12)
    {
        t = t - T / 8 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - K_H1 * H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + K_S * S / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 8 + T / 12 && t < T / 8 + T / 8)
    {
        t = t - T / 8 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - K_H1 * H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + K_S * S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 && t < T / 4 + T / 24)
    {
        t = t - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - K_H1 * H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1)-H1 / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + K_S * S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
    {
        t = t - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - K_H1 * H1;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + K_S * (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
    {
        t = t - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - K_H1 * H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + K_S * S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1);
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1);
    }
    else if (t >= T / 4 + T / 8 && t < T / 2)
    {
        t = t - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else if (t >= T / 2 && t < T / 2 + T / 24)
    {
        t = t - T / 2;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2)-H / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 24 && t < T / 2 + T / 12)
    {
        t = t - T / 2 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 12 && t < T / 2 + T / 8)
    {
        t = t - T / 2 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2);
    }
    else if (t >= T / 2 + T / 8 && t < T / 2 + T / 8 + T / 24)
    {
        t = t - T / 2 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2)-H / 4 * (1 - cos(t * PI / (T / 48)));
    }
    else if (t >= T / 2 + T / 8 + T / 24 && t < T / 2 + T / 8 + T / 12)
    {
        t = t - T / 2 - T / 24 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
    }
    else if (t >= T / 2 + T / 8 + T / 12 && t < T / 2 + T / 4)
    {
        t = t - T / 2 - T / 8 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
    }
    else if (t >= T / 2 + T / 4 && t < T / 2 + T / 4 + T / 8)
    {
        t = t - T / 2 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + (-S + (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_1) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + (-S + (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + (-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_1) - (dz_2) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + (-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + dz_3) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    else
    {
        Dog.legs[0].foot_target_x = LEG1_INIT_X;
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z;
        Dog.legs[1].foot_target_x = LEG0_INIT_X;
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z;
        Dog.legs[2].foot_target_x = LEG3_INIT_X;
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z;
        Dog.legs[3].foot_target_x = LEG2_INIT_X;
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z;
    }
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_zhongxinzuoyi()
{
    float T = (GAIT_T + delta_t);
    float dx_10 = barycenter_left; // LL/6;//重心左移，系数可调
    float dz_10 = barycenter_back; // 0*dz;//重心后移，系数可调
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);
    uprintf("Dog_Gait_zhongxinzuoyi t:%f,s:%f\n\r", t, Dog_time.s);
    Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[0].foot_target_y = LEG1_INIT_Y;
    Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_y = LEG0_INIT_Y;
    Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_y = LEG3_INIT_Y;
    Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_y = LEG2_INIT_Y;
    Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) / 2 * (1 - cos(t * PI / (T / 2)));
}

void Dog_Gait_shangtaijie_zuoyou()
{

    float T = (GAIT_T + delta_t);
    float S = (GAIT_S + delta_z);
    float H = (GAIT_H + delta_h);
    //   float DS = (GAIT_DS + delta_ds);
    //  float Dth = (GAIT_DTH + delta_Dth);
    //  float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);

    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - T), T);

    float h = 60 / 10 * 1.1;
    float H1 = H - 0.0;               //台阶高0.030
    float dx_10 = barycenter_central; // LL / 6;
    float dx_1 = barycenter_left;     // LL / 3;  //重心左移，系数可调
    float dx_2 = barycenter_right;    // LL / 3;  //重心右移，系数可调
    float dz_10 = barycenter_back;    // 0 * dz; //重心后移，系数可调
    float dz_2 = barycenter_front;    // 3 * dz;  //重心前移，系数可调

    //******抬右前*******//
    if (t < T / 24)
    {
        uprintf("---1---\n\r");
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    else if (t >= T / 24 && t < T / 12)
    {
        uprintf("---2---\n\r");
        t = t - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    else if (t >= T / 12 && t < T / 8)
    {
        uprintf("---3---\n\r");
        t = t - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    //******移重心*******//
    else if (t >= T / 8 && t < T / 4)
    {
        uprintf("---4---\n\r");
        t = t - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    //******左前*******//
    else if (t >= T / 4 && t < T / 4 + T / 24)
    {
        uprintf("---5---\n\r");
        t = t - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
    {
        uprintf("---6---\n\r");
        t = t - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
    {
        uprintf("---7---\n\r");
        t = t - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H + H1 / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10);
    }
    //******移重心*******//
    else if (t >= T / 4 + T / 8 && t < T / 4 + T / 4)
    {
        uprintf("---8---\n\r");
        t = t - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y - H + H1 - (-H + H1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y - H + H1 - (-H + H1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) - (dz_2) / 2 * (1 - cos(t * PI / (T / 8)));
    }
    //******左后*******//
    else if (t >= T / 4 + T / 4 && t < T / 4 + T / 4 + T / 24)
    {
        uprintf("---9---\n\r");
        t = t - T / 4 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) - (dz_2)-H / 4 * (1 - cos(t * PI / (T / 48)));
    }
    else if (t >= T / 4 + T / 4 + T / 24 && t < T / 4 + T / 4 + T / 12)
    {
        uprintf("---10---\n\r");
        t = t - T / 4 - T / 4 - T / 24;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
    }
    else if (t >= T / 4 + T / 4 + T / 12 && t < T / 4 + T / 4 + T / 8)
    {
        uprintf("---11---\n\r");
        t = t - T / 4 - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
    }
    //******移重心*******//
    else if (t >= T / 2 + T / 8 && t < T / 2 + T / 4)
    {
        uprintf("---12---\n\r");
        t = t - T / 2 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) - (dx_1) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    //******左后*******//
    else if (t >= T / 2 + T / 4 && t < T / 2 + T / 4 + T / 24)
    {
        uprintf("---13---\n\r");
        t = t - T / 2 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10)-H / 4 * (1 - cos(t * PI / (T / 48))) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    else if (t >= T / 2 + T / 4 + T / 24 && t < T / 2 + T / 4 + T / 12)
    {
        uprintf("---14---\n\r");
        t = t - T / 2 - T / 24 - T / 4;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) + (S) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    else if (t >= T / 2 + T / 4 + T / 12 && t < T / 2 + T / 4 + T / 8)
    {
        uprintf("---15---\n\r");
        t = t - T / 2 - T / 4 - T / 12;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[2].foot_target_y = LEG3_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24))) - (dz_2);
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) - (dx_1);
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2);
    }
    //******移重心*******//
    else
    {
        uprintf("---16---\n\r");
        t = t - T / 2 - T / 4 - T / 8;
        Dog.legs[0].foot_target_x = LEG1_INIT_X + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[0].foot_target_y = LEG1_INIT_Y;
        Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_x = LEG0_INIT_X + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[1].foot_target_y = LEG0_INIT_Y;
        Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_x = LEG3_INIT_X + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[2].foot_target_y = LEG3_INIT_Y;
        Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_x = LEG2_INIT_X + (-dx_10) + (dx_2) - (dx_1) - ((dx_2) - (dx_1)) / 2 * (1 - cos(t * PI / (T / 8)));
        Dog.legs[3].foot_target_y = LEG2_INIT_Y;
        Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz_10) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz_2) + ((dz_2)-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 8)));
    }

    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Gait_centroid_forward()
{
    float T = (GAIT_T + delta_t);
    // float S = (GAIT_S + delta_z);
    // float H = (GAIT_H + delta_h);
    // float DS = (GAIT_DS + delta_ds);
    //     float Dth = (GAIT_DTH + delta_Dth);
    //  float dth = (GAIT_dth + delta_dth);
    float dz = (GAIT_DZ + delta_dz);
    get_tick(&Dog_time);
    float t = fmod((Dog_time.s - 1.5 * T), T);
    // float dz = delta_dz;
    Dog.legs[0].foot_target_x = LEG1_INIT_X;
    Dog.legs[0].foot_target_y = LEG1_INIT_Y;
    Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[1].foot_target_x = LEG0_INIT_X;
    Dog.legs[1].foot_target_y = LEG0_INIT_Y;
    Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[2].foot_target_x = LEG3_INIT_X;
    Dog.legs[2].foot_target_y = LEG3_INIT_Y;
    Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog.legs[3].foot_target_x = LEG2_INIT_X;
    Dog.legs[3].foot_target_y = LEG2_INIT_Y;
    Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 2)));
    Dog_Inverse_kinematics(&Dog.legs[0]);
    Dog_Inverse_kinematics(&Dog.legs[1]);
    Dog_Inverse_kinematics(&Dog.legs[2]);
    Dog_Inverse_kinematics(&Dog.legs[3]);

    Dog_servo_angle_pos();
}

void Dog_Inverse_kinematics1(Leg_s *leg)
{
    float px;
    float py;
    float pz;

    px = leg->foot_target_x;
    py = leg->foot_target_y;
    pz = leg->foot_target_z;

    float la = LEG_LA;
    float lb = LEG_LB;
    float l1 = LEG_L1;
    float l2 = LEG_L2;
    float l3 = LEG_L3;
    float l4 = LEG_L4;

    double th1 = 0;
    double th2_1 = 0;
    double th2_2 = 0;
    double th3_1 = 0;
    double th3_2 = 0;

    th1 = atan2(sqrt(pow(px - la / 2, 2) + pow(py, 2) - pow(l1, 2)), l1) + atan2(py, px - la / 2);
    uprintf("th1 = %f\n", th1);
    double X = px - la / 2;
    double Y = py;
    double Z = pz - lb / 2;
    double M = sin(th1);
    double N = cos(th1);
    double A = l1 * N + l2 * M;
    double B = -l2 * N + l1 * M;
    double K = pow(l4, 2) * pow(N, 2) - pow((B - Y), 2) - pow(Z, 2) * pow(N, 2) - pow(l3, 2) * pow(N, 2);
    double P = 2 * Z * pow(N, 2) * l3;
    double Q = -2 * (B - Y) * l3 * N;

    th2_1 = atan2(K, sqrt(pow(P, 2) + pow(Q, 2) - pow(K, 2))) - atan2(Q, P);
    th2_2 = atan2(K, -sqrt(pow(P, 2) + pow(Q, 2) - pow(K, 2))) - atan2(Q, P);

    th3_1 = asin(-(Z + l3 * sin(th2_1)) / l4) - th2_1;
    th3_2 = asin(-(Z + l3 * sin(th2_2)) / l4) - th2_2;

    leg->servo[0].goal_angle = RAD2ANGLE(th1);
    leg->servo[1].goal_angle = RAD2ANGLE(th2_1);
    leg->servo[2].goal_angle = RAD2ANGLE(th3_1);
}

/**
 * @brief 腿部运动学逆解
 * @param  leg              My Param doc
 */
void Dog_Inverse_kinematics(Leg_s *leg)
{
    float x1;
    float y1;
    float z1;

    x1 = leg->foot_target_x;
    y1 = leg->foot_target_y;
    z1 = leg->foot_target_z;

    // float B1 = LEG_B1;
    // if (leg->No == 1 || leg->No == 2)
    // {
    //     B1 = -LEG_B1;
    // }
    float A1 = LEG_A1;
    float L1 = LEG_L1;
    float L2 = LEG_L2;
    float L3 = LEG_L3;

    float th10 = 0;
    float th20 = 0;
    float th30 = 0;

    float th1_1 = -atan2(x1, y1);
    // float th1_1 = -atan2((x1 - B1), y1);
    if (leg->No == 0 || leg->No == 3)
    {
        th1_1 = -th1_1;
    }

    float A_1 = cos(th1_1) * x1 + sin(th1_1) * y1;
    float A_2 = -sin(th1_1) * x1 + cos(th1_1) * y1;
    float A_3 = z1;
    float Y = A_2 - L1;
    float Z = A_3 - A1;
    float c1 = (pow(Y, 2) + pow(Z, 2) - pow(L2, 2) - pow(L3, 2)) / (2 * L2 * L3);
    float th3_1 = 0;
    if (abs(c1) < 1.0)
    {
        // if (Dog.ctrl_mode == 11 || Dog.ctrl_mode == 13)
        // {
        //     if (leg->No == 0 || leg->No == 1)
        //     {
        //         th3_1 = -acos(c1);
        //     }
        //     else
        //     {
        //         th3_1 = acos(c1);
        //     }
        // }
        // else
        // {
        if (leg->No == 0 || leg->No == 1)
        {
            th3_1 = acos(c1);
        }
        else
        {
            th3_1 = acos(c1);
        }
        // }
    }
    else
    {
        th3_1 = 0;
    }
    double a = A_2 - L1 + L3 * cos(th3_1) + L2;
    double b = 2 * L3 * sin(th3_1);
    double c = A_2 - L1 - L3 * cos(th3_1) - L2;
    double t1 = 0, t2 = 0;
    double delta = pow(b, 2) - 4 * a * c;
    if (delta > 0)
    {
        t1 = (-b - sqrt(delta)) / (2 * a);
        t2 = (-b + sqrt(delta)) / (2 * a);
    }
    else
    {
        t1 = 0;
        t2 = 0;
    }
    float t2_1 = 0;
    if (z1 < A1)
    {
        t2_1 = 2 * atan(t1);
    }
    else if (z1 == A1)
    {
        t2_1 = 2 * atan(-b / (2 * a));
    }
    else
    {
        t2_1 = 2 * atan(t2);
    }
    float th2_1 = t2_1;
    float th[3];
    uprintf("th1_1 %f th2_1 %f th3_1 %f \n\r", th1_1, th2_1, th3_1);
    th[0] = th1_1 + th10;
    th[1] = th2_1 + th20;
    th[2] = th3_1 + th30;
    leg->servo[0].goal_angle = RAD2ANGLE(th1_1 + th10);
    leg->servo[1].goal_angle = RAD2ANGLE(th2_1 + th20);
    leg->servo[2].goal_angle = RAD2ANGLE(th3_1 + th30);
// float th11 = th1_1 + th10;
// float th21 = th2_1 + th20;
// float th31 = th3_1 + th30;
#ifdef PRINT_INSTRUCTION
    uprintf("th11 %f th21 %f th31 %f \n\r", th[0], th[1], th[2]);
#endif
}

/**
 * @brief 更新舵机目标angle后调用此函数
 */
void Dog_servo_angle_pos()
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // Dog.legs[i].servo[j].goal_angle = broadcast_ctrl_angle[i * 3 + j];
            Servo_Angle2Pos_Mappping(&Dog.legs[i].servo[j]);
            broadcast_ctrl_pos[i * 3 + j] = Dog.legs[i].servo[j].goal_pos;
            broadcast_ctrl_angle[i * 3 + j] = Dog.legs[i].servo[j].goal_angle;
        }
    }
}

/**
 * @brief 更新舵机目标pos后调用此函数
 */
void Dog_servo_pos_angle()
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // Dog.legs[i].servo[j].goal_pos = broadcast_ctrl_pos[i * 3 + j];
            Servo_Pos2Angle_Mappping(&Dog.legs[i].servo[j]);
            broadcast_ctrl_angle[i * 3 + j] = Dog.legs[i].servo[j].goal_angle;
            broadcast_ctrl_pos[i * 3 + j] = Dog.legs[i].servo[j].goal_pos;
        }
    }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    switch (GPIO_Pin)
    {
    case GPIO_PIN_4:
        // Servo_Sync_Torque(1);
        // Dog_Gait_Reset();
        // Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        Servo_Sync_ResetLeg(4);
        Dog.ctrl_mode = DOG_MODE_NONE;
        break;

    case GPIO_PIN_5:
        if (HAL_GPIO_ReadPin(S2_GPIO_Port, S2_Pin) == GPIO_PIN_RESET)
        {
            Servo_Sync_Torque(1);
            Dog.ctrl_mode = DOG_MODE_NONE;
        }
        else
        {
            Servo_Sync_Torque(0);
            Dog.ctrl_mode = DOG_MODE_NONE;
        }
        break;

    case GPIO_PIN_6:
        break;
    default:
        break;
    }
}

// /**
//  * @brief 预备右转
//  */
// void Dog_Gait_turn1_1()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     float DS = (GAIT_DS + delta_ds);
//     float Dth = (GAIT_DTH + delta_Dth);
//         float dth = (GAIT_dth + delta_dth);
// float dz = (GAIT_DZ + delta_dz);
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     float B1 = 0.0776 / 2;
//     float B2 = 0.0415;
//     float A1 = LEG_A1;
//     float th00 = atan((B1 + B2) / A1);
//     float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
//     float Z1 = LL * cos(th00 - Dth);
//     float X1 = LL * sin(th00 - Dth);
//     float Dx1 = X1 - B1 - B2;
//     float Dz1 = Z1 - A1;
//     float Z2 = LL * cos(th00 + Dth);
//     float X2 = LL * sin(th00 + Dth);
//     float Dx2 = X2 - B1 - B2;
//     float Dz2 = Z2 - A1;
//     if (t < T / 4)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z;
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z;
//     }
//     else if (t > T / 4 && t < T / 2)
//     {
//         t = t - (T / 4);
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2;
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2;
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//     }
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }
// /**
//  * @brief 右转
//  */
// void Dog_Gait_turn1_2()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     float DS = (GAIT_DS + delta_ds);
//     float Dth = (GAIT_DTH + delta_Dth);
//         float dth = (GAIT_dth + delta_dth);
// float dz = (GAIT_DZ + delta_dz);
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     float B1 = 0.0776 / 2;
//     float B2 = 0.0415;
//     float A1 = LEG_A1;
//     float th00 = atan((B1 + B2) / A1);
//     float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
//     float Z1 = LL * cos(th00 + Dth);
//     float X1 = LL * sin(th00 + Dth);
//     float Dx1 = (X1 - B1 - B2);
//     float Dz1 = (Z1 - A1);
//     float Z2 = LL * cos(th00 - Dth);
//     float X2 = LL * sin(th00 - Dth);
//     float Dx2 = -(X2 - B1 - B2);
//     float Dz2 = (Z2 - A1);
//     if (t < T / 4)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z;
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z;
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) / 2 * (1 - cos((t)*PI / (T / 4)));
//     }
//     else if (t > T / 4 && t < T / 2)
//     {
//         t = t - (T / 4);
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + (-Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1);
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx2) / 2 * (1 - cos((t)*PI / (T / 4)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) / 2 * (1 - cos((t * PI / (T / 4))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1);
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1);
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X - Dx2 - (-Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx2 - (Dx2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) / 2 * (1 - cos((t - T / 2) * PI / (T / 2)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) / 2 * (1 - cos((t + T / 2) * PI / (T / 2)));
//     }
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }
// /**
//  * @brief 预备左转
//  */
// void Dog_Gait_turn2_1()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     float DS = (GAIT_DS + delta_ds);
//     float Dth = (GAIT_DTH + delta_Dth);
//         float dth = (GAIT_dth + delta_dth);
// float dz = (GAIT_DZ + delta_dz);
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     float B1 = 0.0776 / 2;
//     float B2 = 0.0415;
//     float A1 = LEG_A1;
//     float A = 100 * PI / 180;
//     if (Dog_time.s > 15)
//     {
//         A = -100 * PI / 180;
//     }
//     float th00 = atan((B1 + B2) / A1);
//     float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
//     float Dth_1 = (fmod((A / (PI / 180)), (Dth / (PI / 180)))) * PI / 180;
//     // uprintf("Dth %f Dth_1 %f\n\r", Dth, Dth_1);
//     float Z1 = LL * cos(th00 - Dth_1);
//     float X1 = LL * sin(th00 - Dth_1);
//     float Dx1 = X1 - B1 - B2;
//     float Dz1 = Z1 - A1;
//     float Z2 = LL * cos(th00 + Dth_1);
//     float X2 = LL * sin(th00 + Dth_1);
//     float Dx2 = X2 - B1 - B2;
//     float Dz2 = Z2 - A1;
//     float H1 = H - 0.0;
//     if (t < T / 4)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + (Dx2)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z;
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + (-Dx2) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) * 0.5 * (1 - cos((t * PI / (T * 0.25))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z;
//     }
//     else if (t > T / 4 && t < T / 2)
//     {
//         t = t - (T / 4);
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2;
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2;
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + Dx2 - (Dx2)*0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2)*0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) * 0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1)*0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X - Dx2 - (-Dx2) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1)*0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) * 0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//     }
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }
// /**
//  * @brief 左转
//  */
// void Dog_Gait_turn2_2()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     float DS = (GAIT_DS + delta_ds);
//     float Dth = (GAIT_DTH + delta_Dth);
//         float dth = (GAIT_dth + delta_dth);
// float dz = (GAIT_DZ + delta_dz);
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     float B1 = 0.0776 / 2;
//     float B2 = 0.0415;
//     float A1 = LEG_A1;
//     float A = 100 * PI / 180;
//     if (Dog_time.s > 15)
//     {
//         A = -100 * PI / 180;
//     }
//     float th00 = atan((B1 + B2) / A1);
//     float LL = sqrt(pow((B1 + B2), 2) + pow(A1, 2));
//     float Dth_1 = (fmod((A / (PI / 180)), (Dth / (PI / 180)))) * PI / 180;
//     float Z1 = LL * cos(th00 + Dth_1);
//     float X1 = LL * sin(th00 + Dth_1);
//     float Dx1 = (X1 - B1 - B2);
//     float Dz1 = (Z1 - A1);
//     float Z2 = LL * cos(th00 - Dth_1);
//     float X2 = LL * sin(th00 - Dth_1);
//     float Dx2 = -(X2 - B1 - B2);
//     float Dz2 = (Z2 - A1);
//     float H1 = H - 0.0;
//     if (t < T / 4)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z;
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z;
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//     }
//     else if (t > T / 4 && t < T / 2)
//     {
//         t = t - (T / 4);
//         Dog.legs[0].foot_target_x = LEG1_INIT_X + (-Dx2) * 0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (Dz2)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X + (-Dx1);
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (Dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + (Dx2)*0.5 * (1 - cos((t)*PI / (T * 0.25)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H * 0.5 * (1 - cos(t * PI / (T * 0.125)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (-Dz2) * 0.5 * (1 - cos((t * PI / (T * 0.25))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + (Dx1);
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (-Dz1);
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X - Dx2 - (-Dx2) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + Dz2 - (Dz2)*0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X - Dx1 - (-Dx1) * 0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + Dz1 - (Dz1)*0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X + Dx2 - (Dx2)*0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z - Dz2 - (-Dz2) * 0.5 * (1 - cos((t - T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X + Dx1 - (Dx1)*0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z - Dz1 - (-Dz1) * 0.5 * (1 - cos((t + T * 0.5) * PI / (T * 0.5)));
//     }
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }
// /**
//  * @brief 上楼梯1
//  */
// void Dog_Gait_upstairs1()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     // float DS = (GAIT_DS + delta_ds);
//     //     float Dth = (GAIT_DTH + delta_Dth);
//     float dth = (GAIT_dth + delta_dth);
//     float dz = (GAIT_DZ + delta_dz);
//     // float dz = delta_dz; // + sqrt(pow(0.220, 2) - pow(0.030, 2)) / 4;
//     float h = 30 / 10 * 1.1;
//     // uprintf("dz %f h %f\n\r", dz, h);
//     float H1 = H + delta_dh;
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     if (t < T / 8)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//     }
//     else if (t >= T / 8 && t < T / 8 + T / 24)
//     {
//         t = t - T / 8;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz)-H1 / 4 * (1 - cos(t * PI / (T / 48)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 8 + T / 24 && t < T / 8 + T / 12)
//     {
//         t = t - T / 8 - T / 24;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) / 2 * (1 - cos(t * PI / (T / 24)));
//     }
//     else if (t >= T / 8 + T / 12 && t < T / 8 + T / 8)
//     {
//         t = t - T / 8 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//     }
//     else if (t >= T / 4 && t < T / 4 + T / 24)
//     {
//         t = t - T / 4;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) - H1 / 4 * (1 - cos(t * PI / (T / 48)));
//     }
//     else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
//     {
//         t = t - T / 4 - T / 24;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H1;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)))) + (S + (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))))) / 2 * (1 - cos(t * PI / (T / 24)));
//     }
//     else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
//     {
//         t = t - T / 4 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//     }
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }
// /**
//  * @brief 上楼梯
//  */
// void Dog_Gait_upstairs()
// {
//     float T = (GAIT_T + delta_t);
//     float S = (GAIT_S + delta_z);
//     float H = (GAIT_H + delta_h);
//     // float DS = (GAIT_DS + delta_ds);
//     //     float Dth = (GAIT_DTH + delta_Dth);
//     float dth = (GAIT_dth + delta_dth);
//     float dz = (GAIT_DZ + delta_dz);
//     // float dz = delta_dz; //+ sqrt(pow(0.220, 2) - pow(0.030, 2)) / 4;//0.0545
//     float dz1 = delta_dz1;
//     float h = 30 / 10 * 1.1;
//     // uprintf("dz %f h %f\n\r", dz, h);
//     float H1 = H + delta_dh;
//     get_tick(&Dog_time);
//     float t = fmod((Dog_time.s - 1.5 * T), T);
//     if (t < T / 8)
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) / 2 * (1 - cos(t * PI / (T / 8)));
//     }
//     else if (t >= T / 8 && t < T / 8 + T / 24)
//     {
//         t = t - T / 8;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz)-H1 / 4 * (1 - cos(t * PI / (T / 48)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 8 + T / 24 && t < T / 8 + T / 12)
//     {
//         t = t - T / 8 - T / 24;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 8 + T / 12 && t < T / 8 + T / 8)
//     {
//         t = t - T / 8 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 4 && t < T / 4 + T / 24)
//     {
//         t = t - T / 4;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz)-H1 / 4 * (1 - cos(t * PI / (T / 48)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 4 + T / 24 && t < T / 4 + T / 12)
//     {
//         t = t - T / 4 - T / 24;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 4 + T / 12 && t < T / 4 + T / 8)
//     {
//         t = t - T / 4 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y - H1 + H1 / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz);
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz);
//     }
//     else if (t >= T / 4 + T / 8 && t < T / 2)
//     {
//         t = t - T / 4 - T / 8;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//     }
//     else if (t >= T / 2 && t < T / 2 + T / 24)
//     {
//         t = t - T / 2;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1)-H / 4 * (1 - cos(t * PI / (T / 48)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1);
//     }
//     else if (t >= T / 2 + T / 24 && t < T / 2 + T / 12)
//     {
//         t = t - T / 2 - T / 24;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1);
//     }
//     else if (t >= T / 2 + T / 12 && t < T / 2 + T / 8)
//     {
//         t = t - T / 2 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1);
//     }
//     else if (t >= T / 2 + T / 8 && t < T / 2 + T / 8 + T / 24)
//     {
//         t = t - T / 2 - T / 8;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1)-H / 4 * (1 - cos(t * PI / (T / 48)));
//     }
//     else if (t >= T / 2 + T / 8 + T / 24 && t < T / 2 + T / 8 + T / 12)
//     {
//         t = t - T / 2 - T / 24 - T / 8;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1) + (S) / 2 * (1 - cos(t * PI / (T / 24)));
//     }
//     else if (t >= T / 2 + T / 8 + T / 12 && t < T / 2 + T / 4)
//     {
//         t = t - T / 2 - T / 8 - T / 12;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1);
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y - H + H / 2 * (1 - cos(t * PI / (T / 24)));
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) / 2 * (1 - cos(t * PI / (T / 24)));
//     }
//     else if (t >= T / 2 + T / 4 && t < T / 2 + T / 4 + T / 8)
//     {
//         t = t - T / 2 - T / 4;
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1) + (-S + (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - dz + dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z + (dz) + S - (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - (dz1) + (-S + (H1 * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - dz + dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + (-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - dz + dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z + (dz) - (dz1) + S - (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) + (-S + (H * h / sqrt(pow(TAIJIE_PARM, 2) + pow(h, 2))) - dz + dz1) / 2 * (1 - cos(t * PI / (T / 8)));
//     }
//     else
//     {
//         Dog.legs[0].foot_target_x = LEG1_INIT_X;
//         Dog.legs[0].foot_target_y = LEG1_INIT_Y;
//         Dog.legs[0].foot_target_z = LEG1_INIT_Z;
//         Dog.legs[1].foot_target_x = LEG0_INIT_X;
//         Dog.legs[1].foot_target_y = LEG0_INIT_Y;
//         Dog.legs[1].foot_target_z = LEG0_INIT_Z;
//         Dog.legs[2].foot_target_x = LEG3_INIT_X;
//         Dog.legs[2].foot_target_y = LEG3_INIT_Y;
//         Dog.legs[2].foot_target_z = LEG3_INIT_Z;
//         Dog.legs[3].foot_target_x = LEG2_INIT_X;
//         Dog.legs[3].foot_target_y = LEG2_INIT_Y;
//         Dog.legs[3].foot_target_z = LEG2_INIT_Z;
//     };
//     Dog_Inverse_kinematics(&Dog.legs[0]);
//     Dog_Inverse_kinematics(&Dog.legs[1]);
//     Dog_Inverse_kinematics(&Dog.legs[2]);
//     Dog_Inverse_kinematics(&Dog.legs[3]);
//     Dog_servo_angle_pos();
// }

#endif // 0
